#include "modbus_app.h"
#include "io_module.h"
#include "adc_module.h"
#include "temp_module.h"
#include "flow_module.h"
#include "water_module.h"
#include "pid_module.h"
#include "alarm_module.h"
#include "log_module.h"
#include <modbus.h>

static rt_thread_t modbus_thread = RT_NULL;
static modbus_t *modbus_ctx = RT_NULL;

/* Modbus register arrays */
static uint16_t input_registers[512];
static uint16_t holding_registers[512];

static rt_err_t modbus_init(void)
{
    modbus_ctx = modbus_new_rtu(MODBUS_SERIAL_DEVICE, MODBUS_BAUD_RATE, 
                               MODBUS_PARITY, MODBUS_DATA_BITS, MODBUS_STOP_BITS);
    if (modbus_ctx == RT_NULL) {
        LOG_E("Failed to create Modbus context\n");
        return RT_ERROR;
    }
    
    /* Set slave address */
    modbus_set_slave(modbus_ctx, MODBUS_SLAVE_ADDR);
    
    /* Set response timeout */
    modbus_set_response_timeout(modbus_ctx, 1, 0);  /* 1 second */
    
    /* Connect to serial port */
    if (modbus_connect(modbus_ctx) != 0) {
        LOG_E("Failed to connect Modbus\n");
        modbus_free(modbus_ctx);
        return RT_ERROR;
    }
    
    LOG_I("Modbus RTU initialized: device=%s, addr=%d\n", 
          MODBUS_SERIAL_DEVICE, MODBUS_SLAVE_ADDR);
    
    return RT_OK;
}

static rt_err_t update_input_registers(void)
{
    /* Update digital input status */
    for (int i = 0; i < 12; i++) {
        if (io_get_input_valid_state(i)) {
            input_registers[REG_INPUT_STATUS_BASE + i] = 1;
        } else {
            input_registers[REG_INPUT_STATUS_BASE + i] = 0;
        }
    }
    
    /* Update analog inputs (4-20mA values) */
    for (int i = 0; i < 4; i++) {
        float value = adc_get_engineering_value(i);
        input_registers[REG_ANALOG_INPUT_BASE + i] = (uint16_t)(value * 10);  /* Scale by 10 for precision */
    }
    
    /* Update temperatures */
    for (int i = 0; i < 4; i++) {
        float temp = temp_get_temperature(i);
        input_registers[REG_TEMPERATURE_BASE + i] = (uint16_t)(temp * 10);  /* Scale by 10 for precision */
    }
    
    /* Update flow rates */
    for (int i = 0; i < 2; i++) {
        float flow = flow_get_flow_rate(i);
        input_registers[REG_FLOW_BASE + i] = (uint16_t)(flow * 10);  /* Scale by 10 for precision */
    }
    
    /* Update water quality */
    for (int i = 0; i < 2; i++) {
        float quality = water_get_quality_value(i);
        input_registers[REG_WATER_QUALITY_BASE + i] = (uint16_t)(quality * 10);  /* Scale by 10 for precision */
    }
    
    /* Update PID outputs */
    input_registers[REG_PID_OUTPUT_BASE] = (uint16_t)(pid_get_output(PID_HEATER) * 10);
    input_registers[REG_PID_OUTPUT_BASE + 1] = (uint16_t)(pid_get_output(PID_COMPRESSOR) * 10);
    
    return RT_OK;
}

static rt_err_t process_holding_register_write(uint16_t address, uint16_t value)
{
    switch (address) {
        case REG_CONTROL_BASE:
            /* System control */
            if (value & 0x0001) {
                /* Start auto control */
                pid_set_auto_control(RT_TRUE);
                log_write(LOG_OPERATION, MODULE_MODBUS, "Auto control started", 0);
            } else {
                /* Stop auto control */
                pid_set_auto_control(RT_FALSE);
                log_write(LOG_OPERATION, MODULE_MODBUS, "Auto control stopped", 0);
            }
            break;
            
        case REG_SETPOINT_BASE:
            /* Temperature setpoint */
            pid_set_setpoint(PID_HEATER, value / 10.0f);
            log_write(LOG_OPERATION, MODULE_MODBUS, "Setpoint changed", value / 10.0f);
            break;
            
        case REG_PID_PARAM_BASE:
            /* PID parameters */
            {
                float kp = holding_registers[REG_PID_PARAM_BASE] / 1000.0f;
                float ki = holding_registers[REG_PID_PARAM_BASE + 1] / 1000.0f;
                float kd = holding_registers[REG_PID_PARAM_BASE + 2] / 1000.0f;
                pid_set_parameters(PID_HEATER, kp, ki, kd, 0, 100);
            }
            break;
            
        case REG_ALARM_LIMITS_BASE:
            /* Alarm limits */
            {
                float low_limit = holding_registers[REG_ALARM_LIMITS_BASE] / 10.0f;
                float high_limit = holding_registers[REG_ALARM_LIMITS_BASE + 1] / 10.0f;
                alarm_set_limits(0, low_limit, high_limit, ALARM_LEVEL_WARNING);
            }
            break;
            
        default:
            /* Unknown register, ignore */
            break;
    }
    
    return RT_OK;
}

static void modbus_server_loop(void)
{
    uint8_t query[MODBUS_RTU_MAX_ADU_LENGTH];
    int rc;
    
    while (1) {
        /* Wait for request */
        rc = modbus_receive(modbus_ctx, query);
        if (rc > 0) {
            /* Process request */
            modbus_reply(modbus_ctx, query, rc, input_registers, holding_registers);
        } else if (rc == -1) {
            /* Error occurred */
            LOG_E("Modbus receive error\n");
            rt_thread_mdelay(100);
        }
    }
}

rt_err_t modbus_app_init(void)
{
    rt_err_t result;
    
    /* Initialize Modbus */
    result = modbus_init();
    if (result != RT_OK) {
        LOG_E("Modbus initialization failed\n");
        return result;
    }
    
    /* Initialize register arrays */
    rt_memset(input_registers, 0, sizeof(input_registers));
    rt_memset(holding_registers, 0, sizeof(holding_registers));
    
    /* Set default values */
    holding_registers[REG_SETPOINT_BASE] = 250;  /* 25.0°C */
    holding_registers[REG_PID_PARAM_BASE] = 2000;  /* Kp = 2.000 */
    holding_registers[REG_PID_PARAM_BASE + 1] = 100;  /* Ki = 0.100 */
    holding_registers[REG_PID_PARAM_BASE + 2] = 1000;  /* Kd = 1.000 */
    
    /* Create Modbus thread */
    modbus_thread = rt_thread_create("modbus", 
                                    modbus_app_thread_entry, 
                                    RT_NULL, 
                                    2048, 
                                    5, 
                                    20);
    if (modbus_thread == RT_NULL) {
        LOG_E("Failed to create Modbus thread\n");
        modbus_close(modbus_ctx);
        modbus_free(modbus_ctx);
        return RT_ERR_MEM;
    }
    
    LOG_I("Modbus application initialized successfully\n");
    return RT_OK;
}

void modbus_app_thread_entry(void *parameter)
{
    rt_uint32_t last_update = rt_tick_get();
    
    while (1) {
        /* Update input registers periodically */
        if (rt_tick_get() - last_update > 1000) {  /* Update every second */
            update_input_registers();
            last_update = rt_tick_get();
        }
        
        /* Process Modbus requests */
        modbus_server_loop();
        
        rt_thread_mdelay(10);
    }
}

rt_err_t modbus_update_input_registers(void)
{
    return update_input_registers();
}

rt_err_t modbus_process_holding_register(uint16_t address, uint16_t value)
{
    return process_holding_register_write(address, value);
}